
#pragma once
#include "zdt_motor.hpp"
#include "pwm_board.hpp"
#include "code_scan.hpp"
#include "tjc_screen.hpp"
#include <math.h>

/*
       ↑ X+ (前进方向)
       │
       │
┌──────┼──────┐
│ \\1  │  //2 │ ← 车轮辊子方向标识(从上往下俯视)
│  \\  │ //   │    \\ = 左上到右下的辊子
│   ───┼───   │    // = 右上到左下的辊子
│      ↺      │ ← 中心旋转轴 (↺ = 逆时针正转)
│   ───┼───   │
│  //3 │ \\4  │
│ //   │  \\  │
└──────┼──────┘
       │
 Y+ ← ─┘ (左侧方向)

 */
class car
{
public:
    typedef struct
    {
        double wheel1_speed;
        double wheel2_speed;
        double wheel3_speed;
        double wheel4_speed;
        uint8_t motor_aceel;
    } car_wheel_speed_t;

    typedef struct
    {
        double x_speed;
        double y_speed;
        double z_speed;
        uint8_t motor_aceel;
    } car_speed_t;

private:
    zdt_motor *_wheel1, *_wheel2, *_wheel3, *_wheel4;
    zdt_motor *_lift_motor, *_rotate_motor, *_bracket_motor;

    pwm_board *_claw_board;
    code_scan *_scanner;
    tjc_screen *_screen;

    double _Rx, _Ry; // 轮子轴心到车辆轴心的距离在x和y方向上投影的距离,单位mm
    double _wheel_d; // 轮子直径,单位mm

    double _dest_car_x, _dest_car_y, _dest_car_angle;

    // 单位mm/s  r/s
    yj_return _cal_wheel_speed(const car_speed_t &car_speed, car_wheel_speed_t &wheel_speed);

    void _limit_wheel_min_speed(car_wheel_speed_t &wheel_speed);

public:
    car(zdt_motor *wheel1, zdt_motor *wheel2,
        zdt_motor *wheel3, zdt_motor *wheel4,
        zdt_motor *rotate_motor, zdt_motor *lift_motor,
        zdt_motor *bracket_motor, pwm_board *claw_board,
        uint32_t Rx, uint32_t Ry, double wheel_d,
        code_scan *scanner,tjc_screen *screen);

    // 单位mm/s  r/s
    yj_return set_speed(double x_speed, double y_speed, double z_speed, int32_t motor_aceel);
    yj_return set_speed(const car_speed_t &car_speed);

    // step_xx为步进角度或步进距离，正负值决定运动方向 speed决定速度（正负值无影响，仅取绝对值）
    yj_return set_rotate_pos(double step_angle,
                             double speed,
                             uint8_t accel,
                             zdt_motor::motor_pos_mode_en pos_mode = zdt_motor::motor_pos_mode_relative,
                             bool is_waiting = false);

    // 速度正负决定方向
    yj_return set_rotate_vel(double speed, uint8_t accel);

    // step_xx为步进角度或步进距离，正负值决定运动方向 speed决定速度（正负值无影响，仅取绝对值）
    yj_return set_lift_pos(double step_mm,
                           double speed,
                           uint8_t accel,
                           zdt_motor::motor_pos_mode_en pos_mode = zdt_motor::motor_pos_mode_relative,
                           bool is_waiting = false); // 60mm周长

    // 速度正负决定方向
    yj_return set_lift_vel(double speed, uint8_t accel);

    // step_xx为步进角度或步进距离，正负值决定运动方向 speed决定速度（正负值无影响，仅取绝对值）
    yj_return set_bracket_pos(double step_mm,
                              double speed,
                              uint8_t accel,
                              zdt_motor::motor_pos_mode_en pos_mode = zdt_motor::motor_pos_mode_relative,
                              bool is_waiting = false); // 16 pi

    // 速度正负决定方向
    yj_return set_bracket_vel(double speed, uint8_t accel);

    yj_return reset_rotate_pos();
    yj_return reset_lift_pos();
    yj_return reset_bracket_pos();

    yj_return clear_all_wheel_protect_flag();
    yj_return clear_rotate_protect_flag();
    yj_return clear_lift_protect_flag();
    yj_return clear_bracket_protect_flag();

    yj_return set_claw_channel(uint8_t channel, uint8_t value); // channel 1-7(舵机或灯) value 0-100
    yj_return set_claw_led(pwm_board::led_color_en color, uint8_t brightness, pwm_board::led_mode_en mode);

    yj_return read_wheel_speed(car_wheel_speed_t &wheel_speed);
    yj_return read_bracket_speed_and_pos(double &speed, double &pos);
    yj_return read_lift_speed_and_pos(double &speed, double &pos);
    yj_return read_rotate_speed_and_pos(double &speed, double &pos);

    yj_return cal_car_speed(const car_wheel_speed_t &wheel_speed, car_speed_t &car_speed);

    yj_return scan_code();
    yj_return read_color_index_with_round(uint8_t round_index,uint8_t times_index,uint8_t& color_index);

    yj_return get_set_color_sq_screen(tjc_screen::tjc_set_color_en get_witch, uint8_t& get_value);
    bool read_start_flag();

};